/**
 * @file vision.c
 * @author yanshuoyu
 * @brief  接受视觉数据并经过pid计算处理得出yaw轴和pitch轴值，并由视觉控制开火以及单发和连发
 * @version beta
 * @date 2024-10-21
 *
 *
 */

#include "vision.h"
#include "robot_cmd.h"
#include "master_process.h"
#include "seasky_protocol.h"
#include "controller.h"
#include "gimbal.h"
#include "dji_motor.h"
#include "referee_task.h"
#include "shoot.h"
#include "usart.h"
#include "ins_task.h"


// static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据

// RX_PACKET* rxpack;
// PIDInstance vision_PID;
// PIDRefs refs;
// static VisionAngle vision_angle;
// static Vision_Data Vision_angle;
// // vision_Controller_s* vision_controller;

// // 全局标志位，0 表示不开火，1 表示需要开火
// uint8_t vision_fire_flag = 0;
// float flag_vision_ = 0;

// // 初始化裁判系统数据
// static uint16_t robot_hp = 0;
// static uint16_t shooter_heat = 0;
// static uint16_t remaining_shots = 0;
// static float speed = 0;

// 获取裁判系统数据
void update_referee_data()
{
    // if (referee_recv_info->init_flag)
    // {
    //     robot_hp = referee_recv_info->GameRobotState.current_HP;
    //     shooter_heat = referee_recv_info->PowerHeatData.shooter_17mm_1_barrel_heat;
    //     uint16_t heat_limit = referee_recv_info->GameRobotState.shooter_barrel_heat_limit;
    //     remaining_shots = (heat_limit > shooter_heat) ? (heat_limit - shooter_heat) / 10 : 0;
    //     // speed = referee_recv_info->ShootData.bullet_speed;
    // }
}

void GetMeasure(float yaw, float pitch)
{
    // vision_angle.pitch = pitch;
    // vision_angle.yaw = yaw;
}
void GetVision(float yaw, float pitch){
//     refs.pid_ref_y = yaw;
//     refs.pid_ref_p = pitch;
}

// 初始化视觉模块和裁判系统
void vision_to_init()
{   
//     vision_angle.pitch = 0;
//     vision_angle.yaw = 0;
//     refs.pid_ref_p = 0;
//     refs.pid_ref_y = 0;

//     vision_PID = (PIDInstance){
//         .Kp =4,
//         .Ki = 0.5,
//         .Kd = 0,
//         .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
//         .IntegralLimit = 1000,
//         .MaxOut = 2,
//     };
    //疑似bug2，不能重复初始化
    // referee_recv_info = RefereeInit(&huart6); // 裁判系统初始化
    // referee_recv_info->init_flag = 1;
}

// 视觉控制逻辑：控制云台角度并自动开火
PIDRefs vision_to_control()
{
    
    // int found_flag = 0;

    // rxpack =  Get_rxpack();
    // flag_vision_ = *Get_flag();
    // if(rxpack->tracking && flag_vision_ ==1){
    //     // 获取视觉目标角度值
    //     refs.pid_ref_p = *Get_pitch();
    //     refs.pid_ref_y = *Get_yaw();
    // rxpack =  Get_rxpack();
    // flag_vision_ = Get_flag();
    // if(rxpack->tracking && flag_vision_ ==1){
    //     GetVisionData(&Vision_angle);
    //     refs.pid_ref_y = PIDCalculate(&vision_PID, Vision_angle.yaw, 0);
    //     refs.pid_ref_p = PIDCalculate(&vision_PID, Vision_angle.pitch, 0);
    //     rxpack->tracking = 0;
    // }
        // // 更新裁判系统数据
        // update_referee_data();

        // 自动开火逻辑
        // if (robot_hp > 0 && shooter_heat < 300 && remaining_shots > 0)
        // {
          //  vision_fire_flag = 1; // 设置开火标志位
        // }
        // else
        // {
        //     vision_fire_flag = 0; // 清除开火标志位
        // }
        // rxpack->tracking = 0;
    //}
    // else
    // {
    //     vision_fire_flag = 0; // 未找到目标时清除标志位 
    // }
  //  return refs;
    
    
    
    // // 判断目标是否锁定成功
    // if (abs(refs.pid_ref_p) < 1.0 && abs(refs.pid_ref_y) < 1.0)
    // {
    //     found_flag = 1;
    // }

    // if (found_flag)
    // {
    //     // 更新 PID 控制
    //     refs.pid_ref_y = PIDCalculate1(&vision_controller->vision_PID, refs.pid_ref_y, 0);
    //     refs.pid_ref_p = PIDCalculate1(&vision_controller->vision_PID, refs.pid_ref_p, 0);

    //     // 更新裁判系统数据
    //     update_referee_data();

    //     // 自动开火逻辑
    //     if (robot_hp > 0 && shooter_heat < 300 && remaining_shots > 0)
    //     {
    //         vision_fire_flag = 1; // 设置开火标志位
    //     }
    //     else
    //     {
    //         vision_fire_flag = 0; // 清除开火标志位
    //     }
    // }
    // else
    // {
    //     vision_fire_flag = 0; // 未找到目标时清除标志位
    // }

    
}